#include "servoio.h"

rt_device_t device_sbus;
uint8_t sbus_buff[25], sbus_data[25]="";

rt_err_t servo_init(device_servo *servo, char *pwmdev_name, int channel, uint32_t init_value, int offset){
    int i;
    for (i=0; pwmdev_name[i] && i<RT_NAME_MAX; ++i){
        servo->pwmdev_name[i] = pwmdev_name[i];
    }
    servo->pwmdev_name[i] = 0;
    servo->offset = offset;
    servo->configuration.channel = channel;
    servo->configuration.period = SERVO_PWM_PERIOD;
    servo->configuration.pulse = init_value;

    servo->pwmdev = (struct rt_device_pwm*)rt_device_find(pwmdev_name);
    if (servo->pwmdev == RT_NULL){
        return RT_ERROR;
    }

    servo_write(servo, init_value);

    return rt_pwm_enable(servo->pwmdev, channel);
}

rt_err_t servo_write(device_servo *servo, uint32_t value){
    if (value < SERVO_MIN){
        value = SERVO_MIN;
    }else if (value > SERVO_MAX){
        value = SERVO_MAX;
    }

    servo->configuration.pulse = value + servo->offset;
    return rt_device_control(&(servo->pwmdev->parent), PWM_CMD_SET, &(servo->configuration));
}

rt_err_t sbus_event(rt_device_t dev, rt_size_t size){
    static uint8_t *b = sbus_buff;
    rt_device_read(dev, 0, b, 1);
    /*结束位为0x00*/
    if (*b != 0x00){
        ++b;
        /*防止缓冲区溢出*/
        if (b-sbus_buff > 25){
            b = sbus_buff;
            return RT_ERROR;
        }
        return RT_EOK;
    }else{
        b = sbus_buff;
        /*复制数据*/
        for (int i=0; i<25; ++i){
            sbus_data[i] = sbus_buff[i];
        }
        /*校验起始位*/
        if (sbus_buff[0]==0x0f){
            return RT_EOK;
        }else{
            return RT_ERROR;
        }
    }
    return RT_EOK;
}

rt_err_t sbus_init(char *uart_dev_name){
    device_sbus = rt_device_find(uart_dev_name);

    struct serial_configure sbus_conf;
    sbus_conf.baud_rate = 100000;
    sbus_conf.data_bits = DATA_BITS_8;
    sbus_conf.parity = PARITY_EVEN;
    sbus_conf.stop_bits = STOP_BITS_2;
    sbus_conf.bufsz = 25;

    rt_device_control(device_sbus, RT_DEVICE_CTRL_CONFIG, &sbus_conf);
    rt_device_open(device_sbus, RT_DEVICE_FLAG_INT_RX);
    rt_device_set_rx_indicate(device_sbus, sbus_event);
    return RT_EOK;
}

uint16_t sbus_read_channel(uint8_t channel){
    switch (channel){
    case 1:
        return ((int16_t)sbus_data[1] >> 0 | ((int16_t)sbus_data[2] << 8 )) & 0x07FF;
        break;
    case 2:
        return ((int16_t)sbus_data[2] >> 3 | ((int16_t)sbus_data[3] << 5 )) & 0x07FF;
        break;
    case 3:
        return  ((int16_t)sbus_data[3] >> 6 | ((int16_t)sbus_data[4] << 2 )  | (int16_t)sbus_data[5] << 10 ) & 0x07FF;
        break;
    case 4:
        return ((int16_t)sbus_data[5] >> 1 | ((int16_t)sbus_data[6] << 7 )) & 0x07FF;
        break;
    case 5:
        return ((int16_t)sbus_data[6] >> 4 | ((int16_t)sbus_data[7] << 4 )) & 0x07FF;
        break;
    case 6:
        return ((int16_t)sbus_data[7] >> 7 | ((int16_t)sbus_data[8] << 1 )  | (int16_t)sbus_data[9] <<  9 ) & 0x07FF;
        break;
    case 7:
        return ((int16_t)sbus_data[9] >> 2 | ((int16_t)sbus_data[10] << 6 )) & 0x07FF;
        break;
    case 8:
        return ((int16_t)sbus_data[10] >> 5 | ((int16_t)sbus_data[11] << 3 )) & 0x07FF;
        break;
    case 9:
        return ((int16_t)sbus_data[12] << 0 | ((int16_t)sbus_data[13] << 8 )) & 0x07FF;
        break;
    case 10:
        return ((int16_t)sbus_data[13] >> 3 | ((int16_t)sbus_data[14] << 5 )) & 0x07FF;
        break;
    case 11:
        return ((int16_t)sbus_data[14] >> 6 | ((int16_t)sbus_data[15] << 2 )  | (int16_t)sbus_data[16] << 10 ) & 0x07FF;
        break;
    case 12:
        return ((int16_t)sbus_data[16] >> 1 | ((int16_t)sbus_data[17] << 7 )) & 0x07FF;
        break;
    case 13:
        return ((int16_t)sbus_data[17] >> 4 | ((int16_t)sbus_data[18] << 4 )) & 0x07FF;
        break;
    case 14:
        return ((int16_t)sbus_data[18] >> 7 | ((int16_t)sbus_data[19] << 1 )  | (int16_t)sbus_data[20] <<  9 ) & 0x07FF;
        break;
    case 15:
        return ((int16_t)sbus_data[20] >> 2 | ((int16_t)sbus_data[21] << 6 )) & 0x07FF;
        break;
    case 16:
        return ((int16_t)sbus_data[21] >> 5 | ((int16_t)sbus_data[22] << 3 )) & 0x07FF;
        break;
    }
    return 0;
}

uint32_t sbus2servo(uint16_t sbus_val){
    return 625*sbus_val + 875000;
}
//
//void sbus_read(){
//    rt_kprintf("%d\t%d\t%d\t%d\r\n", sbus_read_channel(1), sbus_read_channel(2), sbus_read_channel(3), sbus_read_channel(4));
//}
//
//#ifdef FINSH_USING_MSH
//    MSH_CMD_EXPORT(sbus_read, read s-bus output);
//#endif
